/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: RangingData.cpp,v 1.24 2009-06-04 09:16:50 morsko Exp $
 *
 * \file
 * \brief Ranging interface data encoder/decoder implementation.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "RangingData.hpp"
#include "binbag.h"
#include "owndebug.h"
#include <stdio.h>

//*****************************************************************************
//*****************************************************************************
using namespace MaCI::Ranging;
//*****************************************************************************
//*****************************************************************************

CRangingData::CRangingData()
  : iValid(false),
    iTimestampPtr(NULL),
    iPositionPtr(NULL),
    iLimitsPtr(NULL),
    iDistanceHeaderPtr(NULL),
    iFirstDistancePtr(NULL),
    iFirstReflectionPtr(NULL),
    iDistanceElementCount(0),
    iHeaderDistanceElementCount(0),
    iReflectionElementCount(0),
    iHeaderReflectionElementCount(0),
    iFirstCommandPtr(NULL),
    iCommandCount(0)
{
}
//*****************************************************************************

CRangingData::~CRangingData()
{
}
//*****************************************************************************

CRangingData::CRangingData(const CRangingData &aData)
  : iValid(false),
    iTimestampPtr(NULL),
    iPositionPtr(NULL),
    iLimitsPtr(NULL),
    iDistanceHeaderPtr(NULL),
    iFirstDistancePtr(NULL),
    iFirstReflectionPtr(NULL),
    iDistanceElementCount(0),
    iHeaderDistanceElementCount(0),
    iReflectionElementCount(0),
    iHeaderReflectionElementCount(0),
    iFirstCommandPtr(NULL),
    iCommandCount(0)
{
  // Call assignment operator.
  *this = aData;
}
//*****************************************************************************

CRangingData &CRangingData::operator=(const CRangingData &aData)
{
  // Reset
  Reset();

  // Create an internal BinBag instance
  if (CreateInternalBinBag(aData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aData);
  }
  
  // Do the decode on the buffer; it sets pointers and
  if (DecodeFrom() == false) {
    dPrint(4,"Warning: Failed to Decode CRangingData even though assignment operator used?");
  }
  
  return *this; 
}
//*****************************************************************************
bool CRangingData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
  bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess
  if (aBinBag != NULL) 
    iBinBagContainer = aBinBag;
  
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {
      case KTypeTimestamp:
	if (iTimestampPtr == NULL) {
	  iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);
	} else result = false;
	break;
	
      case KTypeDistanceHeader:
	if (iDistanceHeaderPtr == NULL) {
	  iDistanceHeaderPtr = reinterpret_cast<const TDistanceHeader *>(b->data);
          iHeaderDistanceElementCount = iDistanceHeaderPtr->beam_count;
	} else result = false;
	break;
	
      case KTypeDistance:
	if (iFirstDistancePtr == NULL && iDistanceHeaderPtr != NULL &&
	    (previous_type == KTypeDistanceHeader || previous_type == KTypeDistance || previous_type == KTypeReflection)) {
	  iFirstDistancePtr = reinterpret_cast<const TDistance *>(b->data);
	  iDistanceElementCount ++;
	  
	} else if (iDistanceHeaderPtr != NULL && 
		   (previous_type == KTypeDistanceHeader || previous_type == KTypeDistance || previous_type == KTypeReflection)) {
	  iDistanceElementCount ++;
	  
	} else result = false;
	break;
      case KTypeReflection:
	if (iFirstReflectionPtr == NULL && iDistanceHeaderPtr != NULL &&
	    (previous_type == KTypeDistanceHeader || previous_type == KTypeDistance || previous_type == KTypeReflection)) {
	  iFirstReflectionPtr = reinterpret_cast<const TReflection *>(b->data);
	  iReflectionElementCount ++;
	  
	} else if (iDistanceHeaderPtr != NULL && 
		   (previous_type == KTypeDistanceHeader || previous_type == KTypeDistance || previous_type == KTypeReflection)) {
	  iReflectionElementCount ++;
	  
	} else result = false;
	break;
      case KTypeDeviceInformationLimits: 
	if (iLimitsPtr == NULL) {
	  iLimitsPtr = reinterpret_cast<const TDeviceInformationLimits *>(b->data);
	} else result = false;
	break;
	
      case KTypeDeviceInformationPosition:
	if (iPositionPtr == NULL) {
	  iPositionPtr = reinterpret_cast<const TDeviceInformationPosition *>(b->data);
	} else result = false;
	break;
	
      case KTypeCommand:
	if (iFirstCommandPtr == NULL) {
	  iFirstCommandPtr = reinterpret_cast<const TCommand *>(b->data);
	  iCommandCount ++;

	} else if (previous_type == KTypeCommand) {
	  iCommandCount ++;

	} else result = false;
	break;

      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
      
      // Store previous type
      previous_type = b->type;
    }
  } else result = false;

  // Store the result
  SetReadOnlyContainer( (iValid = result) );

  return result;
}
//*****************************************************************************

void CRangingData::Reset()
{
  CMaCIData::Reset();

  // Clean out. (Same values as in default constructor)
  iValid = (false);

  iTimestampPtr = (NULL);
  iPositionPtr = (NULL);
  iLimitsPtr = (NULL);
  iDistanceHeaderPtr = (NULL);
  iFirstDistancePtr = (NULL);
  iFirstReflectionPtr = (NULL);
  iDistanceElementCount = (0);
  iHeaderDistanceElementCount = 0;
  iReflectionElementCount = (0);
  iHeaderReflectionElementCount = 0;
  iFirstCommandPtr = (NULL);
  iCommandCount = (0);
}
//*****************************************************************************

bool CRangingData::SetTimestamp(const Common::TTimestamp &aStamp)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTimestampPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeTimestamp, 
                                  (const char *)&aStamp, sizeof(Common::TTimestamp));
    iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);

    // Mark last element
    SetLastElementType( KTypeTimestamp);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CRangingData::SetDeviceInformationPosition(const TDeviceInformationPosition &aPos)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iPositionPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeDeviceInformationPosition, (const char *)&aPos, 
				  sizeof(TDeviceInformationPosition));
    iPositionPtr = reinterpret_cast<const TDeviceInformationPosition *>(b->data);

    // Mark last element
    SetLastElementType( KTypeDeviceInformationPosition);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CRangingData::SetDeviceInformationLimits(const TDeviceInformationLimits &aLim)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iLimitsPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeDeviceInformationLimits, (const char *)&aLim, 
				  sizeof(TDeviceInformationLimits));
    iLimitsPtr = reinterpret_cast<const TDeviceInformationLimits *>(b->data);

    // Mark last element
    SetLastElementType( KTypeDeviceInformationLimits);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CRangingData::SetDistanceHeader(const TDistanceHeader &aHdr)
{  
  bool result = false;
  if (!IsReadOnlyContainer() && iDistanceHeaderPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeDistanceHeader, (const char *)&aHdr, 
				  sizeof(TDistanceHeader));
    iDistanceHeaderPtr = reinterpret_cast<const TDistanceHeader *>(b->data);
    iHeaderDistanceElementCount = aHdr.beam_count; // Store for
                                                   // structure
                                                   // integrity checks
    iHeaderReflectionElementCount =aHdr.reflection_count; // Store for
                                                          // structure
                                                          // integrity checks

    // Mark last element
    SetLastElementType( KTypeDistanceHeader);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CRangingData::AddDistance(const TDistance &aDist)
{
  bool result = false;

  // Now, normal checks.
  if (!IsReadOnlyContainer() && 
      iDistanceHeaderPtr != NULL && 
      iDistanceElementCount < iHeaderDistanceElementCount &&
      (GetLastElementType() == KTypeDistanceHeader || GetLastElementType() == KTypeDistance || GetLastElementType() == KTypeReflection)) {
    // 1. Not read-only
    // 2. DistanceHeader is set
    // 3. Added element count is under header's beam_count
    // 4. Element is in sequence (previous is either Distance or DistanceHeader)
    
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeDistance, (const char *)&aDist, 
				  sizeof(TDistance));
    
    if (iFirstDistancePtr == NULL) {
      iFirstDistancePtr = reinterpret_cast<const TDistance *>(b->data);
    }

    // Increment element count
    ++iDistanceElementCount;
    
    // If added element count matches the set header count, set iValid == true
    
    if (iHeaderDistanceElementCount == iDistanceElementCount) iValid = true;

    // Mark last element
    SetLastElementType( KTypeDistance);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CRangingData::AddReflection(const TReflection &aRefl)
{
  bool result = false;

  // Now, normal checks.
  if (!IsReadOnlyContainer() && 
      iDistanceHeaderPtr != NULL && 
      iReflectionElementCount < iHeaderReflectionElementCount &&
      (GetLastElementType() == KTypeDistanceHeader || GetLastElementType() == KTypeDistance || GetLastElementType() == KTypeReflection)) {
    // 1. Not read-only
    // 2. DistanceHeader is set
    // 3. Added element count is under header's beam_count
    // 4. Element is in sequence (previous is either Distance or DistanceHeader)
    
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeReflection, (const char *)&aRefl, 
				  sizeof(TReflection));
    
    if (iFirstReflectionPtr == NULL) {
      iFirstReflectionPtr = reinterpret_cast<const TReflection *>(b->data);
    }

    // Increment element count
    ++iReflectionElementCount;
    
    // If added element count matches the set header count, set iValid == true
    
    if (iHeaderReflectionElementCount == iReflectionElementCount) iValid = true;

    // Mark last element
    SetLastElementType( KTypeReflection);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CRangingData::AddCommand(const TCommand &aCmd)
{
  bool result = false;
  if (!IsReadOnlyContainer() && 
      (iFirstCommandPtr == NULL || GetLastElementType() == KTypeCommand)) {
    // 1. Not readonly
    // 2. First Command pointer is NULL, OR, previous element was command
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeCommand, (const char *)&aCmd, 
				  sizeof(TCommand));
    
    // Set first ptr if not set
    if (iFirstCommandPtr == NULL) {
      iFirstCommandPtr = reinterpret_cast<const TCommand *>(b->data);
    }

    // Increment element count
    ++iCommandCount;

    // Mark last element
    SetLastElementType( KTypeCommand);

    result = true;
  }
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

const MaCI::Common::TTimestamp *CRangingData::GetTimestamp(void) const
{
  return iValid ? iTimestampPtr : NULL;
}
//*****************************************************************************

const TDeviceInformationPosition *CRangingData::GetDeviceInformationPosition(void) const
{
  return iValid ? iPositionPtr : NULL;
}
//*****************************************************************************

const TDeviceInformationLimits *CRangingData::GetDeviceInformationLimits(void) const
{
  return iValid ? iLimitsPtr : NULL;
}
//*****************************************************************************

const TDistanceHeader *CRangingData::GetDistanceHeader(void) const
{
  return iValid ? iDistanceHeaderPtr : NULL;
}
//*****************************************************************************

const TDistance *CRangingData::GetDistance(const int aIndex) const
{
  const TDistance *elem = NULL;
  
  if (iValid) {
    if (aIndex >= 0 && 
	aIndex < iDistanceElementCount && 
	iFirstDistancePtr != NULL) {

      const char *cptr = (const char *)iFirstDistancePtr;
      cptr += aIndex * (sizeof(TDistance) + sizeof(gim::binbag::TBinBlob));
      
      // Now, typecast the address to Element.
      elem = (const TDistance *)cptr;
    }
  }

  return elem;
}
//*****************************************************************************

const TReflection *CRangingData::GetReflection(const int aIndex) const
{
  const TReflection *elem = NULL;
  
  if (iValid) {
    if (aIndex >= 0 && 
	aIndex < iReflectionElementCount && 
	iFirstReflectionPtr != NULL) {

      const char *cptr = (const char *)iFirstReflectionPtr;
      cptr += aIndex * (sizeof(TReflection) + sizeof(gim::binbag::TBinBlob));
      
      // Now, typecast the address to Element.
      elem = (const TReflection *)cptr;
    }
  }

  return elem;
}
//*****************************************************************************

int CRangingData::GetDistanceElementCount() const
{
  return iDistanceElementCount;
}
//*****************************************************************************

int CRangingData::GetReflectionElementCount() const
{
  return iReflectionElementCount;
}
//*****************************************************************************

const TCommand *CRangingData::GetCommand(const int aIndex) const
{
  const TCommand *cmd = NULL;
  if (iValid) {
    if (aIndex == 0) {
      cmd = iFirstCommandPtr;
    } else {
      dPrint(1,"Sorry, no multiple commands currently supported!");
    }
  }
  return cmd;
}
//*****************************************************************************

int CRangingData::GetCommandCount(void) const
{
  if (iValid) {
    return iCommandCount;
  } else return -1;
  
}
//*****************************************************************************

void CRangingData::Print(const int l) const
{ 

  const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeTimestamp: {
	const Common::TTimestamp *ts = 
          reinterpret_cast<const Common::TTimestamp *>(b->data);
	dPrint(l,"TTimestamp : %d, %d", 
	       ts->timestamp_s,
	       ts->timestamp_us);
	break;
      }
	
      case KTypeDistanceHeader: {
	const TDistanceHeader *ts = 
          reinterpret_cast<const TDistanceHeader *>(b->data);
	dPrint(l,"TDistanceHeader: %f, %d,%d", 
	       ts->beam_width,
	       ts->beam_count,
               ts->reflection_count);
	break;
      }
	
      case KTypeDistance: {
	const TDistance *ts = 
          reinterpret_cast<const TDistance *>(b->data);
	dPrint(l,"TDistance : %f, %f", 
	       ts->distance,
	       ts->angle);
	break;
      }
      case KTypeReflection: {
	const TReflection *ts = 
          reinterpret_cast<const TReflection *>(b->data);
	dPrint(l,"TReflection : %f, %f", 
	       ts->reflection,
	       ts->angle);
	break;
      }

      case KTypeDeviceInformationLimits: {
	const TDeviceInformationLimits *ts = 
          reinterpret_cast<const TDeviceInformationLimits *>(b->data);
	dPrint(l,"TDeviceInformationLimits : %f, %f", 
	       ts->max_distance,
	       ts->min_distance);
	break;
      }
	
      case KTypeDeviceInformationPosition: {
	const TDeviceInformationPosition *ts = 
          reinterpret_cast<const TDeviceInformationPosition *>(b->data);
	dPrint(l,"TDeviceInformationPosition : %f, %f, %f", 
	       ts->x,
	       ts->y,
	       ts->z,
	       ts->heading);
	break;
      }
	
      case KTypeCommand: {
	break;
      }
	
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
    }
  }
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#ifdef COMPILE_RANGINGDATA_STANDALONE_MAIN
int main(void)
{
  using namespace MaCI::Ranging;
  gim::binbag::CBinBag bb;
  CRangingData A;
  CRangingData B;

  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);
  printf("Encode: Timestamp(1,2)\n");
  A.SetTimestamp(MaCI::Common::TTimestamp(1, 2));

  printf("Encode: DistanceHeader(666.0, 2)\n");
  A.SetDistanceHeader((TDistanceHeader(666.0, 2,2)));

  printf("Encode: TDistance(0.0, 1.0)\n");
  A.AddDistance(TDistance(0.0, 1.0));

  printf("Encode: TDistance(1.0, 2.0)\n");
  A.AddDistance(TDistance(1.0, 2.0));

  printf("Encode: TReflection(2.0, 3.0)\n");
  A.AddReflection(TReflection(2.0,3.0));

  printf("Encode: TReflection(3.0, 4.0)\n");
  A.AddReflection(TReflection(3.0,4.0));
  
  printf("Decoding...\n");
  // Do Decoding
  B.DecodeFrom(&bb);
  printf("Decode: Timestamp = (%d, %d)\n", 
	 B.GetTimestamp()->timestamp_s, 
	 B.GetTimestamp()->timestamp_us);

  printf("Decode: DistanceHeader = (%f, %d,%d)\n",
	 B.GetDistanceHeader()->beam_width, 
	 B.GetDistanceHeader()->beam_count,
         B.GetDistanceHeader()->reflection_count);

  printf("Decode: GetDistanceElementCount (%d)\n",
         B.GetDistanceElementCount());

  printf("Decode: Distance(0) = (%f, %f)\n",
	 B.GetDistance(0)->angle,
	 B.GetDistance(0)->distance);

  printf("Decode: Distance(1) = (%f, %f)\n",
	 B.GetDistance(1)->angle,
	 B.GetDistance(1)->distance);

  printf("Decode: GetReflectionElementCount (%d)\n",
         B.GetReflectionElementCount());

  printf("Decode: Reflection(0) = (%f, %f)\n",
	 B.GetReflection(0)->angle,
	 B.GetReflection(0)->reflection);

  printf("Decode: Reflection(1) = (%f, %f)\n",
	 B.GetReflection(1)->angle,
	 B.GetReflection(1)->reflection);
  
  printf("Test complete.\n");
  

}
#endif
//*****************************************************************************
//*****************************************************************************
